// -*- coding: utf-8 -*-
// MCU按键控制的OFFBOARD飞行任务系统
#include "mission.h"
#include "commander.h"
#include "chassis.h"
#include "mcu_data_sender.h"
#include "mcu_data_subscriber.h"
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Float64.h>
#include <locale>
#include <codecvt>

// 创建基础飞行任务
void createBasicMission(WaypointManager &manager)
{
    ROS_WARN("=== 创建基础飞行任务 ===");
    manager.reset();
    
    // 1. 启用OFFBOARD模式并解锁飞机
    manager.addOffboardEnable("initialize_offboard_and_arm");
    manager.addWait(1.0, "stabilize_after_offboard");
    
    // 2. 基础飞行路径
    manager.addMovePoint(0.0, 0.0, 0.75, "takeoff_to_height");
    manager.addMovePoint(0.0, 0.0, 1.5, "takeoff_to_height");
    manager.addHover(3.0, "hover_after_takeoff");
    manager.addMovePoint(1.0, 0.0, 1.5, "move_to_point_1");
    manager.addMovePoint(1.0, 1.0, 1.5, "move_to_point_2");
    manager.addMovePoint(0.0, 1.0, 1.5, "move_to_point_3");
    manager.addMovePoint(0.0, 0.0, 1.5, "move_to_point_4");
    
    // 3. 安全降落
    manager.addSafeLanding("safe_landing");
    
    manager.finalize();
    ROS_WARN("基础飞行任务创建完成！");
}

void createAdvancedMission(WaypointManager &manager)
{
    ROS_WARN("=== 创建高级飞行任务 ===");
    manager.reset();

    // 1. 启用OFFBOARD模式并解锁飞机
    manager.addOffboardEnable("offboard_enable_and_arm");
    manager.addWait(1.0, "stabilize_after_offboard");
    
    // 2. 高级飞行路径：围杆任务
    manager.addMovePoint(0.0, 0.0, 0.75, "takeoff_to_height");
    manager.addMovePoint(0.0, 0.0, 1.5, "takeoff_to_height");
    manager.addMovePoint(2.0, 0.0, 1.5, "approach_pole_area");
    manager.addPoleSurround(3.0, 0.0, 1.0, 1.5, false, "performPoleSurrounding");
    manager.addMovePoint(3.5, 0.0, 1.5, "return_home");
    manager.addRotation(0.0, "scan_front");
    manager.addMovePoint(0.0, 0.0, 1.5, "return_home");

    // 3. 安全降落
    manager.addSafeLanding("safe_landing");

    manager.finalize();
    ROS_WARN("高级飞行任务创建完成！");
}

// 在接收到"switch":"land"命令后创建此任务
void createLandMission(WaypointManager &manager)
{
    manager.reset();
    manager.addWait(3.0, "brake");
    manager.addSafeLanding("safe_landing");
    manager.finalize();
}

void createResetMission(WaypointManager &manager)
{
    manager.reset();
    manager.addOffboardEnable("offboard_enable_and_arm");
    manager.finalize();
}

// MCU按键控制的主程序
int main(int argc, char **argv)
{
    // 设置中文环境
    std::setlocale(LC_ALL, "zh_CN.UTF-8");
    
    ros::init(argc, argv, "mcu_controlled_drone");
    ros::NodeHandle nh;
    
    ROS_INFO("=== MCU按键控制无人机系统启动 ===");
    
    // 初始化MAVROS相关发布器和订阅器
    state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);
    pose_sub = nh.subscribe<geometry_msgs::PoseStamped>("mavros/local_position/pose", 10, pose_cb);
    vel_sub = nh.subscribe<geometry_msgs::TwistStamped>("mavros/local_position/velocity_local", 10, vel_cb);
    
    local_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);
    local_vel_pub = nh.advertise<geometry_msgs::Twist>("mavros/setpoint_velocity/cmd_vel_unstamped", 10);
    
    arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
    set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");

    // 初始化MCU数据订阅器
    McuDataSender mcuSender(nh);

    // 初始化MCU数据订阅器
    McuDataSubscriber mcuSubscriber(nh);
    
    // 等待MAVROS连接
    ros::Rate rate(20.0);
    
    ROS_INFO("等待MAVROS连接...");
    while (ros::ok() && !current_state.connected) {
        ros::spinOnce();
        rate.sleep();
    }
    ROS_INFO("MAVROS已连接！");
    
    // 创建任务管理器和执行器
    WaypointManager waypointManager;
    waypointManager.setNodeHandle(&nh);
    move_map move1;
    
    ROS_INFO("=== 等待MCU按键输入 ===");
    ROS_INFO("basic按键 (值=1.0) -> 执行基础飞行任务");
    ROS_INFO("advanced按键 (值=1.0) -> 执行高级飞行任务");
    ROS_INFO("请确保serialport_node正在运行: rosrun pegasus serialport_node");
    
    // 任务状态变量
    bool missionActive = false;
    std::string activeMissionType = ""; // basic/advanced
    std::string switchMissionType = ""; // 其他执行basic/advanced任务时瞬时切换的任务类型
    
    // RGB发送定时器
    ros::Time lastRgbSendTime = ros::Time::now();
    ros::Duration rgbSendInterval(3.0); // 3秒间隔变换灯的颜色
    
    // 主循环：等待按键输入并执行相应任务
    while (ros::ok()) {
        ros::spinOnce();

        // 每5秒发送一次RGB信号
        ros::Time currentTime = ros::Time::now();
        if (currentTime - lastRgbSendTime >= rgbSendInterval) {
            if (!missionActive) mcuSender.sendJsonString("RGB", "GREEN");
            else if (activeMissionType == "basic") mcuSender.sendJsonString("RGB", "BLUE");
            else if (activeMissionType == "advanced") mcuSender.sendJsonString("RGB", "RED");
            else if (activeMissionType == "land") mcuSender.sendJsonString("RGB", "YELLOW");
            lastRgbSendTime = currentTime;
            ROS_DEBUG("发送RGB信号到MCU");
        }

        // 如果当前没有任务在执行，检查按键输入
        if (!missionActive) {
            // 检查basic按键
            double basicValue = mcuSubscriber.getDoubleValue("basic");
            double advancedValue = mcuSubscriber.getDoubleValue("advanced");
            
            if (basicValue == 1.0 && activeMissionType != "basic") {
                ROS_INFO("检测到basic按键按下！开始执行基础飞行任务...");
                createBasicMission(waypointManager);
                missionActive = true;
                activeMissionType = "basic";
            }
            else if (advancedValue == 1.0 && activeMissionType != "advanced") {
                ROS_INFO("检测到advanced按键按下！开始执行高级飞行任务...");
                createAdvancedMission(waypointManager);
                missionActive = true;
                activeMissionType = "advanced";
            }
            else {
                // 没有按键按下，继续等待
                ROS_INFO_THROTTLE(5.0, "等待按键输入... (basic=%.1f, advanced=%.1f)", 
                                 basicValue, advancedValue);
            }
        }
        
        // 如果有任务在执行，继续执行任务
        if (missionActive && waypointManager.hasNextWaypoint()) {
            Waypoint current = waypointManager.getCurrentWaypoint();
            
            ROS_INFO_THROTTLE(1.0, "[%s任务] 执行步骤: %s (类型: %d)", 
                            activeMissionType.c_str(),
                            current.stepName.c_str(), 
                            static_cast<int>(current.actionType));
            
            bool actionComplete = waypointManager.isCurrentActionComplete(move1, local_vel_pub, rate);
            
            if (actionComplete) {
                ROS_INFO("[%s任务] 步骤完成: %s", activeMissionType.c_str(), current.stepName.c_str());
                waypointManager.moveToNextWaypoint();
            }
        }

        // 处理执行任务时即时切换任务的逻辑
        switchMissionType = mcuSubscriber.getStringValue("switch");
        if (missionActive && waypointManager.hasNextWaypoint() && switchMissionType != activeMissionType) {
            if (switchMissionType == "land") {
                ROS_INFO("检测到切换到降落任务，切换任务类型...");
                createLandMission(waypointManager);
                missionActive = true;
                activeMissionType = "land";
            }
            if (switchMissionType == "reset") {
                ROS_INFO("检测到重置任务，切换任务类型...");
                createResetMission(waypointManager);
                mcuSubscriber.publishDoubleData("basic", 0.0);
                mcuSubscriber.publishDoubleData("advanced", 0.0);
                mcuSubscriber.publishStringData("switch", "");
                missionActive = false;
                activeMissionType = "reset";
            }
        }
        
        // 检查任务是否完成
        if (missionActive && !waypointManager.hasNextWaypoint()) {
            ROS_INFO("=== %s任务完成 ===", activeMissionType.c_str());
            ROS_INFO("等待下一个按键输入...");
            mcuSubscriber.publishDoubleData("basic", 0.0);
            mcuSubscriber.publishDoubleData("advanced", 0.0);
            missionActive = false;
            activeMissionType = "";
        }
        
        rate.sleep();
    }
    
    ROS_INFO("MCU按键控制系统退出");
    return 0;
}
